# LOTS OF Blob Detection
import sensor, image, time,pyb
from pyb import Servo
from pyb import UART
from pyb import Pin, Timer,LED

# 如果要保证颜色追踪效果的话， 需要对环境的严格控制
# 晚上光源的冷暖色等，都会对颜色追踪造成很大的影响

# 彩色图片颜色的阈值格式组成， 是由LAB颜色空间的各自最小值与最大值组成
# 点击右侧的颜色空间下拉选择按钮， 默认为RGB Color Space
# 参考右侧的LAB Color Space里面的参数
# （minL, maxL, minA, maxA, minB, maxB）
# 灰度图的阈值格式
# (min, max)
Red_led = pyb.LED(1) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4.
Green_led = pyb.LED(2)
Blue_led = pyb.LED(3)

s1 = Servo(1) # P7 左右控制
s2 = Servo(2) # P8 上下控制

#def tick(timer):
    #Green_led.toggle()

#tim = Timer(2, freq=1)      # 使用定时器2创建定时器对象-以1Hz触发  T=1/f f=2时，T=1/2=0.5s

#tim.callback(tick)          # 将回调设置为tick函数



uart = UART(3, 115200)   #设置串口和波特率
# 红色阈值
red_threshold =((0, 100, 9, 127, 2, 60))
# 黄色阈值
yellow_threshold = (20, 84, -20, 27, 24, 64)
# 蓝色阈值
blue_threshold = (30, 53, -19, 19, -48, -15)
# 绿色阈值
green_threshold =(0, 100, -29, -10, -21, 26)

# 颜色阈值的设定可以在 工具(Tools) -> 机器视觉(Machine Vision) -> 阈值编辑器(Threshold Editor) 中调试

# 颜色代码是find_blobs返回的blob对象中的一个成分， 用于标识，该色块是由在哪个阈值下选择的
# 颜色1: 红色的颜色代码
red_color_code = 1 # code = 2^0 = 1
# 颜色2: 黄色的颜色代码
yellow_color_code = 2 # code = 2^1 = 2
# 颜色3蓝的代码
blue_color_code = 4# color_code_3 = 2^2 = 4
# 颜色4绿的代码
green_color_code = 8# color_code_4 = 2^3 = 8

red=0
yellow=0
blue=0
green=0

RedTrueCount=0
BlueTrueCount=0
FalseCount=0

receive=""

rec_code1 = "OK"

Start_Find = 0

sensor.reset() # 初始化摄像头
sensor.set_pixformat(sensor.RGB565) # 选择像素模式 RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(time = 2000) # Let new settings take affect.
sensor.set_auto_whitebal(False) #关闭白平衡。白平衡是默认开启的，在颜色识别中，需要关闭白平衡。


clock = time.clock() # Tracks FPS.

s1.angle(180) #舵机1转动到0°
s2.angle(0   ) #舵机1转动到0°

while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    if (uart.any()):
        receive=(uart.read())       #串口接收数据
        print(receive)
        if((receive == "Stop_Find") == 0):
            Blue_led.toggle()
            Start_Find = 0
        if((receive == "Start_Find") == 0):
            Blue_led.toggle()
            Start_Find = 1

    if(Start_Find == 1):
        img = sensor.snapshot() # 拍照，返回图像
        # 在图像中寻找满足颜色阈值约束(color_threshold, 数组格式), 像素阈值pixel_threshold， 色块面积大小阈值(area_threshold)的色块

        blobs = img.find_blobs([red_threshold, yellow_threshold , blue_threshold , green_threshold,], area_threshold=200)
        if blobs:
        #如果找到了目标颜色
            for blob in blobs:
            #迭代找到的目标颜色区域
                x = blob[0]
                y = blob[1] #

                center_x = blob[5] # 色块中心点x值417

                center_y = blob[6] # 色块中心点y值

                color_code = blob[8] # 颜色代码

                if(Start_Find == 1):
                    if color_code == red_color_code:
                        img.draw_string(x, y - 10, "red", color = (0xFF, 0x00, 0x00))
                        red=1
                        #print("Find_red")
                        #Red_led.on()
                   # else:
                        #Red_led.off()


                    if color_code == blue_color_code:
                        img.draw_string(x, y+5, "blue", color = (0x00, 0xff, 0xff))
                        #Blue_led.on()
                        blue=1
                        #print("Find_blue")
                    #else:
                        #Blue_led.off()


                    if color_code == yellow_color_code:
                        img.draw_string(x, y , "yellow", color = (0xff, 0xff, 0x00))
                        yellow=1
                       # print("Find_yellow")


                    if color_code == green_color_code:
                        img.draw_string(x, y +5, "green", color = (0x00, 0xff, 0x00))
                        #Green_led.on()
                        green=1
                        #print("Find_green")
                   # else:
                        #Green_led.off()

                    if red ==1 and yellow == 1:
                        red=0
                        yellow=0
                        blue=0
                        green=0
                        Green_led.on()
                        Red_led.off()
                        Blue_led.off()
                        print("找到伪宝藏")
                        uart.write("FalseCount-")

                    elif red ==1 and green == 1:
                        red=0
                        yellow=0
                        blue=0
                        green=0
                        Green_led.off()
                        Red_led.on()
                        Blue_led.off()
                        print("找到红队真宝藏")
                        uart.write("RedTrueCount-")

                    elif blue ==1 and yellow == 1:
                        red=0
                        yellow=0
                        blue=0
                        green=0
                        Green_led.off()
                        Red_led.off()
                        Blue_led.on()
                        print("找到蓝队真宝藏")
                        uart.write("BlueTrueCount-")

                    elif blue ==1 and green == 1:
                        red=0
                        yellow=0
                        blue=0
                        green=0
                        Green_led.on()
                        Red_led.off()
                        Blue_led.off()
                        print("找到伪宝藏")
                        uart.write("FalseCount-")
                    else:
                        Green_led.off()
                        Red_led.off()
                        Blue_led.off()

    #print(clock.fps())
